#!/usr/bin/env python3
# import rospy
# from drone_pkg.msg import QRCode
# import cv2
# import numpy as np
# from pyzbar.pyzbar import decode
 


# def Read_Decode_Pic(name, image, CamID):
#     global pub
#     for code in decode(image):
#         data = code.data.decode('utf-8')
#         print(data) 
#         pts_poly= np.array(code.polygon, np.int32)  
#         cv2.polylines(image, [pts_poly], True, (0, 255, 0), 5)  
#         pts_rect = code.rect
#         cv2.putText(image, data, (pts_rect[0],pts_rect[1]), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 0, 255), 2)
#         msg = QRCode()
#         msg.Data = str(data)
#         msg.CamID = CamID
#         msg.x = pts_rect[0]
#         msg.y = pts_rect[1]
#         msg.Width = pts_rect[2]
#         msg.Height = pts_rect[3]
#         pub.publish(msg)
#     cv2.imshow(name, image)    
 
 
 
# if __name__ == '__main__':            
#     rospy.init_node("QRCode_detect", anonymous=True)
#     pub = rospy.Publisher("/QRCode_detect", QRCode, queue_size=10)
#     rate = rospy.Rate(100)
#     cap0 = cv2.VideoCapture('/dev/came0')
#     cap1 = cv2.VideoCapture('/dev/came1')
#     while not rospy.is_shutdown():
#         ret, frame0 = cap0.read()
#         ret, frame1 = cap1.read()
#         try:
#             Read_Decode_Pic('0', frame0, 0)     
#             Read_Decode_Pic('1', frame1, 1) 
#         except:
#             print("error")

#         cv2.waitKey(1)
#         rate.sleep()
#     cv2.destroyAllWindows()       
#     cv2.waitKey(0)


import rospy
from drone_pkg.msg import QRCode
import cv2
import numpy as np
from pyzbar.pyzbar import decode
import os
import time




def Laser_Init():
        os.system("gpio mode 10 out")
        os.system("gpio mode 13 out")
        os.system("gpio write 10 1")
        os.system("gpio write 13 1")

def Laser_Control(laser_id,state):
    if laser_id == 1 and state == 0:
        os.system("gpio write 10 1")
    if laser_id == 1 and state == 1:
        os.system("gpio write 10 0")
    if laser_id == 0 and state == 0:
        os.system("gpio write 13 1")
    if laser_id == 0 and state == 1:
        os.system("gpio write 13 0")

def Laser_ON(laser_id, t):
     Laser_Control(laser_id, 1)
     time.sleep(t * 0.1)
     Laser_Control(laser_id, 0)

def shutdown():
     Laser_Control(1, 0)
     Laser_Control(0, 0)
     cv2.destroyAllWindows()  

def Read_Decode_Pic(name, image, CamID):
    global pub
    max_area = 0
    max_code = None
    for code in decode(image):
        data = code.data.decode('utf-8')
        pts_poly = np.array(code.polygon, np.int32)  
        cv2.polylines(image, [pts_poly], True, (0, 255, 0), 5)  
        pts_rect = code.rect
        
        # 计算二维码的面积
        area = pts_rect[2] * pts_rect[3]
        
        # 找到面积最大的二维码
        if area > max_area:
            max_area = area
            max_code = {
                'data': data,
                'pts_rect': pts_rect,
                'cam_id': CamID
            }

    if max_code:
        # 获取最大的二维码的相关信息
        data = max_code['data']
        pts_rect = max_code['pts_rect']
        CamID = max_code['cam_id']
        rospy.loginfo("%d, %d", int(CamID), int(data))
        # 标记最大二维码
        cv2.putText(image, data, (pts_rect[0], pts_rect[1]), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 0, 255), 2)
        
        # 发送最大二维码的信息
        msg = QRCode()
        msg.Data = str(data)
        msg.CamID = CamID
        msg.x = pts_rect[0]
        msg.y = pts_rect[1]
        msg.Width = pts_rect[2]
        msg.Height = pts_rect[3]
        pub.publish(msg)
    
    cv2.imshow(name, image)    


if __name__ == '__main__':
    rospy.init_node("QRCode_detect", anonymous=True)
    rospy.on_shutdown(shutdown)
    Laser_Init()
    pub = rospy.Publisher("/QRCode_detect", QRCode, queue_size=10)
    rate = rospy.Rate(100)
    cap0 = cv2.VideoCapture('/dev/came0')
    cap1 = cv2.VideoCapture('/dev/came1')
    ret, frame0 = cap0.read()
    ret, frame1 = cap1.read()
    # frame0 = cv2.cvtColor(frame0, cv2.COLOR_BGR2GRAY)
    # frame1 = cv2.cvtColor(frame1, cv2.COLOR_BGR2GRAY)
    # frame0 = cv2.equalizeHist(frame0)
    # frame1 = cv2.equalizeHist(frame1)
    try:
        Read_Decode_Pic('0', frame0, 0)     
        Read_Decode_Pic('1', frame1, 1) 
        Laser_Control(0, 1)
        Laser_Control(1, 1)
        time.sleep(2)
        Laser_Control(0, 0)
        Laser_Control(1, 0)
    except Exception as e:
        print(f"Error: {e}")
        


    while not rospy.is_shutdown():
        ret, frame0 = cap0.read()
        ret, frame1 = cap1.read()
        # frame0 = cv2.cvtColor(frame0, cv2.COLOR_BGR2GRAY)
        # frame1 = cv2.cvtColor(frame1, cv2.COLOR_BGR2GRAY)
        # frame0 = cv2.equalizeHist(frame0)
        # frame1 = cv2.equalizeHist(frame1)
        try:
            Read_Decode_Pic('0', frame0, 0)     
            Read_Decode_Pic('1', frame1, 1) 
        except Exception as e:
            print(f"Error: {e}")

        cv2.waitKey(1)
        rate.sleep()
    cv2.destroyAllWindows()       
